﻿using System;
using System.Collections.Generic;
using System.Collections;
using System.Drawing;
using OmologList = MonoPhysics.SimplifiedCircularLinkedListNode<MonoPhysics.GeometryHelper.PointDWithOmologous>;
namespace MonoPhysics
{
    /// <summary>
    /// Moteur physique.
    /// </summary>
    public static class GeometryHelper
    {
        public static PointD GetCentroid(List<PointD> poly, float aire)
        {
            int l = poly.Count;
            float ax = 0, ay = 0;
            int j = l - 1;
            for (int i = 0; i < l; i++)
            {
                ax += (poly[j].X + poly[i].X) * (poly[j].X * poly[i].Y - poly[j].Y * poly[i].X);
                ay += (poly[j].Y + poly[i].Y) * (poly[j].X * poly[i].Y - poly[j].Y * poly[i].X);
                j = i;
            }
            return new PointD(ax / (6 * -aire), ay / (6 * -aire));
        }
        public static Point GetCentroid(List<Point> poly, float aire)
        {
            int l = poly.Count;
            float ax = 0, ay = 0;
            int j = l - 1;
            for (int i = 0; i < l; i++)
            {
                ax += (poly[j].X + poly[i].X) * (poly[j].X * poly[i].Y - poly[j].Y * poly[i].X);
                ay += (poly[j].Y + poly[i].Y) * (poly[j].X * poly[i].Y - poly[j].Y * poly[i].X);
                j = i;
            }
            /*
            Point res = new Point(0, 0);
            if (pts != null && pts.Count != 0)
            {
                int l = pts.Count;
                foreach (Point pt in pts)
                {
                    res.X += pt.X;
                    res.Y += pt.Y;
                }
                res.X /= l;
                res.Y /= l;
            }
             * //*/
            return new Point(Convert.ToInt32(ax / (6 * aire)), Convert.ToInt32(ay / (6 * aire)));
        }

        public static RectangleF GetMinimalRect(List<PointD> pts)
        {
            if (pts == null || pts.Count <= 0)
                return new RectangleF(0, 0, 0, 0);
            else
            {
                float minx = float.MaxValue;
                float maxx = float.MinValue;

                float miny = float.MaxValue;
                float maxy = float.MinValue;

                foreach (PointD p in pts)
                {
                    if (minx > p.X)
                        minx = p.X;
                    if (maxx < p.X)
                        maxx = p.X;
                    if (miny > p.Y)
                        miny = p.Y;
                    if (maxy < p.Y)
                        maxy = p.Y;
                }
                return new RectangleF((float)(minx - 2), (float)(miny - 2), (float)(maxx - minx + 2), (float)(maxy - miny + 2));
            }
        }

        public static Rectangle GetMinimalRect(List<Point> pts)
        {
            if (pts == null || pts.Count <= 0)
                return new Rectangle(0, 0, 0, 0);
            else
            {
                int minx = int.MaxValue;
                int maxx = int.MinValue;

                int miny = int.MaxValue;
                int maxy = int.MinValue;

                foreach (Point p in pts)
                {
                    if (minx > p.X)
                        minx = p.X;
                    if (maxx < p.X)
                        maxx = p.X;
                    if (miny > p.Y)
                        miny = p.Y;
                    if (maxy < p.Y)
                        maxy = p.Y;
                }
                return new Rectangle(minx - 2, miny - 2, maxx - minx + 2, maxy - miny + 2);
            }
        }


        public static void GetMinimalRectAndExtremum(CircularLinkedListNode<PointD, RigidBody> pts, ref CircularLinkedListNode<PointD, RigidBody> safeFusionPoint, ref BroadBox b)
        {
            RigidBody rb = pts.innerParent;
            float minx = float.MaxValue;
            float maxx = float.MinValue;

            float miny = float.MaxValue;
            float maxy = float.MinValue;
            CircularLinkedListNode<PointD, RigidBody> parc = pts;
            do
            {
                if (minx > parc.value.X)
                {
                    safeFusionPoint = parc;
                    minx = parc.value.X;
                }
                if (maxx < parc.value.X)
                    maxx = parc.value.X;
                if (miny > parc.value.Y)
                    miny = parc.value.Y;
                if (maxy < parc.value.Y)
                    maxy = parc.value.Y;
                parc = parc.nodes[1].fils;

            }
            while (pts != parc);

            b.own = rb;
            b.min[0].own = b;
            b.min[0].value = (float)minx;
            b.min[0].isMin = true;
            b.min[1].own = b;
            b.min[1].value = (float)miny;
            b.min[1].isMin = true;
            b.max[0].own = b;
            b.max[0].value = (float)maxx;
            b.max[0].isMin = false;
            b.max[1].own = b;
            b.max[1].value = (float)maxy;
            b.max[1].isMin = false;

        }


        private static List<PointD> SimplifyPointSet(List<PointD> poly, int simplification)
        {
            float xscale = 0;
            float yscale = 0;
            int l = poly.Count;
            int j = l - 1;
            List<PointD> res = new List<PointD>();
            res.Add(poly[j]);
            for (int i = 0; i < l; i++)
            {
                PointD pj = poly[j];
                PointD pi = poly[i];
                xscale += Math.Abs(pj.X - pi.X);
                yscale += Math.Abs(pj.Y - pi.Y);
                if (xscale + yscale > simplification)
                {
                    xscale = 0;
                    yscale = 0;
                    res.Add(pi);
                }
                j = i;
            }
            return res;
        }

        public struct PointDWithOmologous
        {
            public PointD pt;
            public SimplifiedCircularLinkedListNode<PointDWithOmologous> omologue;
            public PointDWithOmologous(PointD pt, SimplifiedCircularLinkedListNode<PointDWithOmologous> omologue)
            {
                this.pt = pt;
                this.omologue = omologue;
            }
        }

        public static bool isPolygonCrossed(List<PointD> pts)
        {
            OmologList lls;
            OmologList sentinelle = new OmologList(new PointDWithOmologous(new PointD(0, 0), null));
            List<PointD> res = new List<PointD>();
            bool stoprecurtion = true;
            int l = pts.Count;
            pts = SimplifyPointSet(pts, 10);
            foreach (PointD pt in pts)
            {
                sentinelle.InsertAfter(new PointDWithOmologous(pt, null));
                sentinelle = sentinelle.Next;
            }
            sentinelle.DeleteNext(); // on supprime le prochain qui se trouve etre la sentinelle!
            lls = sentinelle.Next;
            OmologList parc = lls;
            OmologList limit = lls.Parent;
            do
            {
                SegmentD<OmologList> s1 = new SegmentD<OmologList>(parc.value.pt, parc.Next.value.pt, parc);
                OmologList parc2 = parc;
                parc2 = parc2.Next;
                List<OmologList> intersectsList = new List<OmologList>();
                while (parc2.Next != limit)
                {
                    parc2 = parc2.Next; // on fait ca pour empecher de tester les deux segments adjascents au courant: il ne risquent pas de le coupper!
                    SegmentD<OmologList> s2 = new SegmentD<OmologList>(parc2.value.pt, parc2.Next.value.pt, parc2);
                    PointD p1, p2;
                    if (GeometryHelper.Get2SegmentsIntersection(s1, s2, out p1, out p2, 0.00000000001f) == 1)
                    {       // Inutille de verifier si l'intersection de fait a droite ou a gauche,on s'en fiche car de toutes manieres on y passe une seule fois donc pas de doublons ;)!
                        if (p1 == s1.Pt1 || p1 == s1.Pt2 || p1 == s2.Pt1 || p1 == s2.Pt2)
                            continue;
                        else if (new Vector(p1, s1.Pt1).isNull(0.1f, true) || new Vector(p1, s1.Pt2).isNull(0.1f, true) || new Vector(p1, s2.Pt2).isNull(0.1f, true) || new Vector(p1, s2.Pt2).isNull(0.1f, true))
                            continue;
                        // On ajoute dans la liste d'intersections
                        return true;
                    }
                }
                parc = parc.Next;
                limit = lls; // pour le premier on doit absolument s'arreter a lls.parent sinon on se mort la queue. Ensuite, on peut aller jusqu'a lls.
            }
            while (parc != lls.Parent.Parent);
            return false;
        }

        public static List<PointD> GetUncrossedPolygonList(List<PointD> pts)
        {
            OmologList lls;
            OmologList sentinelle = new OmologList(new PointDWithOmologous(new PointD(0, 0), null));
            List<PointD> res = new List<PointD>();
            bool stoprecurtion = true;
            int l = pts.Count;

            pts = SimplifyPointSet(pts, 10);
            foreach (PointD pt in pts)
            {
                sentinelle.InsertAfter(new PointDWithOmologous(pt, null));
                sentinelle = sentinelle.Next;
            }
            sentinelle.DeleteNext(); // on supprime le prochain qui se trouve etre la sentinelle!
            lls = sentinelle.Next;
            // On fait un petit lifting pour supprimer les lignes droites inutiles!
            // On parcourt directement avec lls car il risque de changer de valeur (si jamais on le suprimme aussi...)
            /*do
            {
                l--; // dans tous les cas on aura une iteration en moins a faire!
                SegmentD<OmologList> olist = new SegmentD<OmologList>(lls.value.pt, lls.Next.Next.value.pt, lls);
                if (olist.ContainsAnypoint(lls.Next.value.pt))
                    lls.DeleteNext();
                else
                    lls = lls.Next;

            }//*/
            //while (l >= 0); // Je sais pas quoi mettre pou le moment...

            // 
            // Recherche des points d'intersections (methode de bourrin en n(n+1)/2 :S)
            OmologList parc = lls;
            OmologList limit = lls.Parent;
            do
            {
                SegmentD<OmologList> s1 = new SegmentD<OmologList>(parc.value.pt, parc.Next.value.pt, parc);
                OmologList parc2 = parc;
                parc2 = parc2.Next;
                List<OmologList> intersectsList = new List<OmologList>();
                while (parc2.Next != limit)
                {
                    parc2 = parc2.Next; // on fait ca pour empecher de tester les deux segments adjascents au courant: il ne risquent pas de le coupper!
                    SegmentD<OmologList> s2 = new SegmentD<OmologList>(parc2.value.pt, parc2.Next.value.pt, parc2);
                    PointD p1, p2;
                    if (GeometryHelper.Get2SegmentsIntersection(s1, s2, out p1, out p2, 0.00000000001f) == 1)
                    {       // Inutille de verifier si l'intersection de fait a droite ou a gauche,on s'en fiche car de toutes manieres on y passe une seule fois donc pas de doublons ;)!
                        if (p1 == s1.Pt1 || p1 == s1.Pt2 || p1 == s2.Pt1 || p1 == s2.Pt2)
                            continue;
                        else if (new Vector(p1, s1.Pt1).isNull(0.1f, true) || new Vector(p1, s1.Pt2).isNull(0.1f, true) || new Vector(p1, s2.Pt2).isNull(0.1f, true) || new Vector(p1, s2.Pt2).isNull(0.1f, true))
                            continue;
                        // On ajoute dans la liste d'intersections
                        intersectsList.Add(new OmologList(new PointDWithOmologous(p1, parc2.Next))); // on inserera les homologues apres.
                        stoprecurtion = false;
                    }
                }
                OmologList nextParc = parc.Next;
                foreach (OmologList ol in intersectsList)
                {
                    float dx = parc.value.pt.X - nextParc.value.pt.X;
                    float dy = parc.value.pt.Y - nextParc.value.pt.Y;
                    float squateMaxDist = dx * dx + dy * dy;
                    OmologList olparc = parc;
                    while (olparc.Next != nextParc)
                    {
                        float thisdx = parc.value.pt.X - olparc.value.pt.X;
                        float thisdy = parc.value.pt.Y - olparc.value.pt.Y;
                        if (thisdx * thisdx + thisdy * thisdy > squateMaxDist)
                            break;
                        olparc = olparc.Next;
                    }
                    olparc.InsertAfter(ol);

                    // Il faut aussi inserer a leurs places les points homologues!
                    // Il n'y ura toujours qu'un point homologue par droite car: deux droites ne peuvent se couper qu'en un point!
                    /*
                    OmologList homoParc = ol.value.omologue;
                    OmologList nexthomoParc = homoParc.Parent;

                    dx = homoParc.value.pt.X - nexthomoParc.value.pt.X;
                    dy = homoParc.value.pt.Y - nexthomoParc.value.pt.Y;
                    squateMaxDist = dx * dx + dy * dy;
                    OmologList olhomoParc = homoParc;
                    while (olhomoParc.Parent != nexthomoParc)
                    {
                        float thisdx = homoParc.value.pt.X - olhomoParc.value.pt.X;
                        float thisdy = homoParc.value.pt.Y - olhomoParc.value.pt.Y;
                        float otherdx = homoParc.value.pt.X - homoParc.Parent.value.pt.X;
                        float otherdy = homoParc.value.pt.Y - homoParc.Parent.value.pt.Y;
                        if (thisdx * thisdx + thisdy * thisdy > otherdx * otherdx + otherdy * otherdy)
                            break;
                        olhomoParc = olhomoParc.Parent;
                    }//*/
                    PointDWithOmologous pwo = new PointDWithOmologous(ol.value.pt, ol);
                    OmologList s = new OmologList(pwo);
                    ol.value.omologue.Parent.InsertAfter(s);
                    ol.value.omologue = s;

                }
                parc = nextParc;
                limit = lls; // pour le preier on doit absolument s'arreter a lls.parent sinon on se mort la queue. Ensuite, on peut aller jusqu'a lls.
            }
            while (parc != lls.Parent.Parent);
            //
            // On ordonne le spoints pou refletter le decroisement total ou partiel:
            parc = lls;
            int le = 0;
            do
            {
                le++;/*
                if (parc.value.omologue != null) // on est sur une borne: il faut faire les iversions
                {
                    OmologList omolog = parc.value.omologue;
                    OmologList invertPart = parc;
                    parc.value.omologue.value.omologue= null;
                    parc.value.omologue = null;
                    while (invertPart.Next != omolog && invertPart != omolog)
                    {
                        // On doit carrement swapper les noeuds (car sinon les autres pointeurs d'omologues seront fausses!
                        OmologList.Swap(invertPart.Next,omolog.Parent );
                        omolog = omolog.Parent;
                        invertPart = invertPart.Next;
                    }
                    // on efface le statut d'instabilite (le fait qu'il y ait des homologues//*/
                /* }
                 //*/
                parc = parc.Next;
            }
            while (parc != lls);
            ptOmologTmp[] ptmp = new ptOmologTmp[le];
            int i = 0;
            do
            {
                if (ptmp[i] != null)
                {
                    parc = parc.Next;
                    i++;
                    continue;
                }
                if (parc.value.omologue != null) // on est sur une borne: il faut faire les iversions
                {
                    int j = i;
                    OmologList pcoo = parc;
                    do
                    {
                        pcoo = pcoo.Next;
                        j++;
                    }
                    while (pcoo != parc.value.omologue);
                    ptmp[i] = new ptOmologTmp(parc.value.pt, j);
                    ptmp[j] = new ptOmologTmp(parc.value.omologue.value.pt, i);
                }
                else
                {
                    ptmp[i] = new ptOmologTmp(parc.value.pt, -1);
                }
                parc = parc.Next;
                i++;
            }
            while (parc != lls);
            for (int p = 0; p < le; p++)
            {
                int hoid = ptmp[p].homoId;
                if (hoid != -1)
                {
                    for (int inv = p + 1; inv < (p + hoid) / 2; inv++)
                    {
                        ptOmologTmp tmpppo = ptmp[inv];
                        if (ptmp[inv].homoId != -1)
                        {
                            ptmp[ptmp[inv].homoId].homoId = hoid - inv + p;
                        }
                        if (ptmp[hoid - inv + p].homoId != -1)
                        {
                            ptmp[ptmp[hoid - inv + p].homoId].homoId = inv;
                        }
                        ptmp[inv] = ptmp[hoid - inv + p];
                        ptmp[hoid - inv + p] = tmpppo;
                    }
                    ptmp[hoid].homoId = -1;
                    ptmp[p].homoId = -1;
                }
            }

            int js0 = le - 2;
            int js1 = 1;
            for (int last = 0; last < le; last++)
            {
                PointD p1 = ptmp[js0].pt;
                PointD pt = ptmp[last].pt;
                PointD p2 = ptmp[js1 % le].pt;
                SegmentD<OmologList> se = new SegmentD<OmologList>(p1, p2, null);
                if (!se.ContainsAnypoint(pt))
                {
                    res.Add(ptmp[last].pt);
                }
                js0 = last;
                js1++;
            }
            //
            // enfin, on retrensforme tout ca en une liste "interative":
            //OmologList lastParc = lls;
            /*
            do
            {
                res.Add(lastParc.value.pt);
                lastParc = lastParc.Next;
            }
            while (lastParc != lls);//*/

            res = SimplifyPointSet(res, 10);

            return (stoprecurtion) ? res : GetUncrossedPolygonList(res);
        }

        private class ptOmologTmp
        {
            public PointD pt;
            public int homoId;
            public ptOmologTmp(PointD pt, int id)
            {
                this.pt = pt;
                this.homoId = id;
            }
            public override string ToString()
            {
                return pt.X.ToString() + " | " + pt.Y.ToString() + " | -> " + homoId.ToString();
            }
        }

        private struct SegmentsIntersectionCouple
        {
            public SegmentD<object> s1;
            public SegmentD<object> s2;
            public PointD inter;
            public SegmentsIntersectionCouple(SegmentD<object> s1, SegmentD<object> s2, PointD inter)
            {
                this.inter = inter;
                this.s1 = s1;
                this.s2 = s2;
            }
        }

        public static bool PointDinPolygon(Polygon p, Vector pos, PointD pt, float delta)
        {
            bool penetrating = false;

            bool good = true;
            LinkedListBlackCallback<PointD, RigidBody> cbk;

            if (delta > 0)
            {
                cbk = delegate(PointD pt1, PointD pt2)
            {
                pt1.X += pos.X;
                pt1.Y += pos.Y;
                pt2.X += pos.X;
                pt2.Y += pos.Y;
                if (delta > 0)
                {
                    float dits = MathGeometryHelper.SqDistPtSeg(pt1, pt2, pt);
                    if (Math.Abs(dits) < delta)// contact point/arette
                    {
                        good = false;
                    }
                }
                if (((pt1.Y < pt.Y && pt2.Y >= pt.Y) || (pt2.Y < pt.Y && pt1.Y >= pt.Y))
                            && ((float)pt1.X + ((float)((float)(pt.Y - pt1.Y)) / ((float)(pt2.Y - pt1.Y))) * (float)(pt2.X - pt1.X) < (float)pt.X))
                    penetrating = !penetrating;

                return true;
            };
            }
            else
            {
                cbk = delegate(PointD pt1, PointD pt2)
                    {
                        pt1.X += pos.X;
                        pt1.Y += pos.Y;
                        pt2.X += pos.X;
                        pt2.Y += pos.Y;
                        if (((pt1.Y < pt.Y && pt2.Y >= pt.Y) || (pt2.Y < pt.Y && pt1.Y >= pt.Y))
                                    && ((float)pt1.X + ((float)((float)(pt.Y - pt1.Y)) / ((float)(pt2.Y - pt1.Y))) * (float)(pt2.X - pt1.X) < (float)pt.X))
                            penetrating = !penetrating;

                        return true;
                    };
            }

            p.Points.Parcours_Noir(cbk);
            return good && penetrating;
        }

        // alogorithme Liang–Barsky: http://en.wikipedia.org/wiki/Liang%E2%80%93Barsky & http://www.cs.helsinki.fi/group/goa/viewing/leikkaus/intro.html
        private delegate bool ClippingHandler(float p, float q);
        public static bool ClipLine(SegmentD line, RectangleF r)
        {
            Vector _clipMin = new Vector(new PointD(0, 0), new PointD(r.X, r.Y));
            Vector _clipMax = new Vector(new PointD(0, 0), new PointD(r.X + r.Width, r.Y + r.Height));
            Vector P = new Vector(line.Pt2 - line.Pt1);
            float tMinimum = 0, tMaximum = 1;

            ClippingHandler pqClip = delegate(float directedProjection,
                                              float directedDistance)
            {
                if (directedProjection == 0)
                {
                    if (directedDistance < 0) return false;
                }
                else
                {
                    float amount = directedDistance / directedProjection;
                    if (directedProjection < 0)
                    {
                        if (amount > tMaximum) return false;
                        else if (amount > tMinimum) tMinimum = amount;
                    }
                    else
                    {
                        if (amount < tMinimum) return false;
                        else if (amount < tMaximum) tMaximum = amount;
                    }
                }
                return true;
            };

            return pqClip(-P.X, line.Pt1.X - _clipMin.X) && pqClip(P.X, _clipMax.X - line.Pt1.X) &&
                   pqClip(-P.Y, line.Pt1.Y - _clipMin.Y) && pqClip(P.Y, _clipMax.Y - line.Pt1.Y);
        }

        public static int Get2SegmentsIntersection<T>(SegmentD<T> s1, SegmentD<T> s2, out PointD IntersecPoint, out PointD endIntersectSeg, float epsilon)
        {
            IntersecPoint = new PointD();
            endIntersectSeg = new PointD();

            Vector u = new Vector(s1.Pt1, s1.Pt2);
            Vector v = new Vector(s2.Pt1, s2.Pt2);
            Vector w = new Vector(s2.Pt1, s1.Pt1);
            float D = u.perp(v);

            if (Math.Abs(D) <= epsilon)
            {
                if (u.perp(w) != 0 || v.perp(w) != 0)
                    return 0;

                float du = u.dot(u);
                float dv = v.dot(v);
                if (du == 0 && dv == 0)
                {
                    if (s1.Pt1 != s2.Pt1)
                        return 0;
                    IntersecPoint = s1.Pt1;
                    return 1;
                }
                if (du == 0)
                {
                    if (!s2.Contains(s1.Pt1))
                        return 0;
                    IntersecPoint = s1.Pt1;
                    return 1;
                }
                if (dv == 0)
                {
                    if (!s1.Contains(s2.Pt1))
                        return 0;
                    IntersecPoint = s2.Pt1;
                    return 1;
                }
                float t0, t1;
                Vector w2 = new Vector(s2.Pt1, s1.Pt2);
                if (v.X != 0)
                {
                    t0 = w.X / v.X;
                    t1 = w2.X / v.X;
                }
                else
                {
                    t0 = -w.Y / v.Y;
                    t1 = -w2.Y / v.Y;
                }

                if (t0 > t1)
                    MathGeometryHelper.swap<float>(ref t1, ref t0);
                if (t0 > 1 || t1 < 0)
                    return 0;
                t0 = (t0 < 0) ? 0 : t0;
                t1 = (t1 > 1) ? 1 : t1;
                if (t0 == t1)
                {
                    IntersecPoint = s2.Pt1 + t0 * v;
                    return 1;
                }

                IntersecPoint = s2.Pt1 + t0 * v;
                endIntersectSeg = s2.Pt1 + t1 * v;
                return 2;
            }
            float sI = v.perp(w) / D;
            if (sI < 0 || sI > 1)
                return 0;

            float tI = u.perp(w) / D;
            if (tI < 0 || tI > 1)
                return 0;
            IntersecPoint = s1.Pt1 + sI * u;
            return 1;
        }
        public static int Get2SegmentsIntersection(SegmentD s1, SegmentD s2, out PointD IntersecPoint, out PointD endIntersectSeg, float epsilon)
        {
            IntersecPoint = new PointD();
            endIntersectSeg = new PointD();

            Vector u = new Vector(s1.Pt1, s1.Pt2);
            Vector v = new Vector(s2.Pt1, s2.Pt2);
            Vector w = new Vector(s2.Pt1, s1.Pt1);
            float D = u.perp(v);

            if (Math.Abs(D) <= epsilon)
            {
                if (u.perp(w) != 0 || v.perp(w) != 0)
                    return 0;

                float du = u.dot(u);
                float dv = v.dot(v);
                if (du == 0 && dv == 0)
                {
                    if (s1.Pt1 != s2.Pt1)
                        return 0;
                    IntersecPoint = s1.Pt1;
                    return 1;
                }
                if (du == 0)
                {
                    if (!s2.Contains(s1.Pt1))
                        return 0;
                    IntersecPoint = s1.Pt1;
                    return 1;
                }
                if (dv == 0)
                {
                    if (!s1.Contains(s2.Pt1))
                        return 0;
                    IntersecPoint = s2.Pt1;
                    return 1;
                }
                float t0, t1;
                Vector w2 = new Vector(s2.Pt1, s1.Pt2);
                if (v.X != 0)
                {
                    t0 = w.X / v.X;
                    t1 = w2.X / v.X;
                }
                else
                {
                    t0 = -w.Y / v.Y;
                    t1 = -w2.Y / v.Y;
                }

                if (t0 > t1)
                    MathGeometryHelper.swap<float>(ref t1, ref t0);
                if (t0 > 1 || t1 < 0)
                    return 0;
                t0 = (t0 < 0) ? 0 : t0;
                t1 = (t1 > 1) ? 1 : t1;
                if (t0 == t1)
                {
                    IntersecPoint = s2.Pt1 + t0 * v;
                    return 1;
                }

                IntersecPoint = s2.Pt1 + t0 * v;
                endIntersectSeg = s2.Pt1 + t1 * v;
                return 2;
            }
            float sI = v.perp(w) / D;
            if (sI < 0 || sI > 1)
                return 0;

            float tI = u.perp(w) / D;
            if (tI < 0 || tI > 1)
                return 0;
            IntersecPoint = s1.Pt1 + sI * u;
            return 1;
        }

        public static void GetSphereTreeCollisions(List<SphereCollision> res, BinaryTree<Circle> a1, BinaryTree<Circle> a2)
        {/*
            Stack<BinaryTree<Circle>> branchesToGo = new Stack<BinaryTree<Circle>>();
            branchesToGo.Push(a1);
            branchesToGo.Push(a2);
            int loops = 0;

            while (branchesToGo.Count != 0)
            {
                loops++;
                    // on pop le 2 avant le 1 car pile.
                BinaryTree<Circle> s2 = branchesToGo.Pop();
                BinaryTree<Circle> s1 = branchesToGo.Pop();
                if (s1.fd == null && s2.fd == null)
                {
                    res.Add(new SphereCollision(s1.Value, s2.Value));
                }
                else
                {
                    BinaryTree<Circle> toSplit;
                    BinaryTree<Circle> theOver;
                    if (s1.fd == null) // alors s2 est point float
                    {
                        toSplit = s2;
                        theOver = s1;
                    }
                    else if (s2.fd == null)
                    {
                        toSplit = s1;
                        theOver = s2;
                    }
                    else // ils sont tous less deux des points doubles!
                    {
                        if (s1.Value.radius <= s2.Value.radius)
                        {
                            toSplit = s2;
                            theOver = s1;
                        }
                        else
                        {
                            toSplit = s1;
                            theOver = s2;
                        }
                    }
                    if (theOver.Value.isIntersect(toSplit.fg.Value))
                    {
                        branchesToGo.Push(theOver);
                        branchesToGo.Push(toSplit.fg);
                    }
                    if (theOver.Value.isIntersect(toSplit.fd.Value))
                    {
                        branchesToGo.Push(theOver);
                        branchesToGo.Push(toSplit.fd);
                    }
                }
            }
            //*/
            if (a1.fd == null && a2.fd == null)
            {
                res.Add(new SphereCollision(a1.Value, a2.Value));
            }
            else
            {
                BinaryTree<Circle> toSplit;
                BinaryTree<Circle> theOver;
                if (a1.fd == null) // alors a2 est point float
                {
                    toSplit = a2;
                    theOver = a1;
                }
                else if (a2.fd == null)
                {
                    toSplit = a1;
                    theOver = a2;
                }
                else // ils sont tous less deux des points doubles!
                {
                    if (a1.Value.radius <= a2.Value.radius)
                    {
                        toSplit = a2;
                        theOver = a1;
                    }
                    else
                    {
                        toSplit = a1;
                        theOver = a2;
                    }
                }
                if (theOver.Value.isIntersect(toSplit.fg.Value))
                    GetSphereTreeCollisions(res, theOver, toSplit.fg);
                if (theOver.Value.isIntersect(toSplit.fd.Value))
                    GetSphereTreeCollisions(res, theOver, toSplit.fd);
            }//*/
        }


        public static bool IsSphereTreeColluding(BinaryTree<Circle> a1, BinaryTree<Circle> a2)
        {
            if (a1.fd == null && a2.fd == null)
                return true;
            else
            {
                BinaryTree<Circle> toSplit;
                BinaryTree<Circle> theOver;
                if (a1.fd == null) // alors a2 est point float
                {
                    toSplit = a2;
                    theOver = a1;
                }
                else if (a2.fd == null)
                {
                    toSplit = a1;
                    theOver = a2;
                }
                else // ils sont tous less deux des points doubles!
                {
                    if (a1.Value.radius <= a2.Value.radius)
                    {
                        toSplit = a2;
                        theOver = a1;
                    }
                    else
                    {
                        toSplit = a1;
                        theOver = a2;
                    }
                }
                bool res = false;
                if (theOver.Value.isIntersect(toSplit.fg.Value))
                    res = IsSphereTreeColluding(theOver, toSplit.fg);
                if (res)
                    return res;
                if (theOver.Value.isIntersect(toSplit.fd.Value))
                    res = IsSphereTreeColluding(theOver, toSplit.fd);
                return res;
            }//*/
        }
        /* Fonctionne PAS!!!!!!
        // http___www.owlnet.rice.edu_~comp450_papers_quinlan_icra94.pdf
        public static void GetMinimumSeparatingSeg(ref SegmentD res, BinaryTree<Circle> s1, BinaryTree<Circle> s2)
        {
            if (s1.fd == null && s2.fd == null)
            {
                if (res == null || res.Length < s1.Value.DistanceTo(s2.Value))
                    res = new SegmentD(s1.Value.Center + s1.Value.Pere.Pos, s2.Value.Center + s2.Value.Pere.Pos);
            }
            else
            {
                BinaryTree<Circle> toSplit;
                BinaryTree<Circle> theOver;
                if (s1.fd == null) // alors s2 est point float
                {
                    toSplit = s2;
                    theOver = s1;
                }
                else if (s2.fd == null)
                {
                    toSplit = s1;
                    theOver = s2;
                }
                else
                {
                    if (s1.Value.radius <= s2.Value.radius)
                    {
                        toSplit = s2;
                        theOver = s1;
                    }
                    else
                    {
                        toSplit = s1;
                        theOver = s2;
                    }
                }
                if ((theOver.Value.DistanceTo(toSplit.fg.Value) <= theOver.Value.DistanceTo(toSplit.fd.Value)))
                    GetMinimumSeparatingSeg(ref res, theOver, toSplit.fg);
                else
                    GetMinimumSeparatingSeg(ref res, theOver, toSplit.fd);
            }
        }

        // for debug
        public static int H = 0;
        public static void DEBUGGetMinimumSeparatingSeg(List<SegmentD> res, BinaryTree<Circle> s1, BinaryTree<Circle> s2, int h)
        {
            if (s1.fd == null && s2.fd == null)
            {
                res.Add(new SegmentD(s1.Value.Center + s1.Value.Pere.Pos, s2.Value.Center + s2.Value.Pere.Pos));
            }
            else
            {
                BinaryTree<Circle> toSplit;
                BinaryTree<Circle> theOver;
                if (s1.fd == null) // alors s2 est point float
                {
                    toSplit = s2;
                    theOver = s1;
                }
                else if (s2.fd == null)
                {
                    toSplit = s1;
                    theOver = s2;
                }
                else
                {
                    if (s1.Value.radius <= s2.Value.radius)
                    {
                        toSplit = s2;
                        theOver = s1;
                    }
                    else
                    {
                        toSplit = s1;
                        theOver = s2;
                    }
                }
                //if ((theOver.Value.DistanceTo(toSplit.fg.Value) <= theOver.Value.DistanceTo(toSplit.fd.Value)))
                DEBUGGetMinimumSeparatingSeg(res, theOver, toSplit.fg, h + 1);
                //else
                DEBUGGetMinimumSeparatingSeg(res, theOver, toSplit.fd, h + 1);
            }
        }
        */


        private static float MinkDistPtDrt(PointD ptd1, PointD ptd2, PointD pt)
        {
            float x1 = ptd1.X;
            float x2 = ptd2.X;
            float res;
            if (x1 == x2) // on a y = a
            {
                res = Math.Abs(x1 - pt.X);
            }
            else
            {
                float y1 = ptd1.Y;
                float y2 = ptd2.Y;
                float a = (y1 - y2) / (x1 - x2);
                float b = y2 - x2 * a;
                float d = (float)Math.Sqrt((float)(a * a + 1));
                if (a == 0) // on a y = b
                {
                    res = Math.Abs(b - pt.Y);
                }
                else
                {
                    res = Math.Abs(a * pt.X - pt.Y + b) / d;
                }
            }
            return res;
        }

        /*
        public static Vector MinkowskiDiff(RigidBody c1, RigidBody c2, ref bool penetre, bool needDID, Vector Direction)
        {
            if (!(c1.Shape is Polygon && c2.Shape is Polygon))
                throw new NotImplementedException("MinkowskiDiff : Ne sais pas calculer la distance de penetration entre des formes non polygonales.");
            CircularLinkedListNode<PointD, RigidBody> pt1 = ((Polygon)c1.Shape).Points;
            CircularLinkedListNode<PointD, RigidBody> pt2 = ((Polygon)c2.Shape).Points;

            CircularLinkedListNode<PointD, RigidBody> parc1 = pt1;
            CircularLinkedListNode<PointD, RigidBody> parc2 = pt2;

            List<PointD> diffPts = new List<PointD>();
            do
            {
                do
                {
                    diffPts.Add(new PointD((float)((parc1.value.X + c1.Pos.X) - (parc2.value.X + c2.Pos.X)), (float)((parc1.value.Y + c1.Pos.Y) - (parc2.value.Y + c2.Pos.Y))));
                    parc2 = parc2.nodes[1].fils;
                }
                while (parc2 != pt2);
                parc1 = parc1.nodes[1].fils;
            }
            while (parc1 != pt1);
            ArrayList alist = ConveXhull(diffPts);
            float maxScalar = float.MaxValue;
            int j = alist.Count - 1;
            PointD org = new PointD(0, 0);
            Vector normal = new Vector();
            penetre = true;
            if (!needDID)
            {
                for (int i = 0; i < alist.Count; i++)
                {
                    Vector v1 = new Vector((PointD)alist[j], org);
                    Vector v2 = new Vector((PointD)alist[i], (PointD)alist[j]);
                    float sc = MinkDistPtDrt((PointD)alist[j], (PointD)alist[i], org);
                    float crossZ = v1.cross(v2).Z;
                    penetre = penetre && crossZ < 0;
                    if (!penetre) return new Vector();

                    if (sc < maxScalar)
                    {
                        maxScalar = sc;
                        normal = v2.direction();
                        normal = new Vector(-normal.Y, normal.X, 0);
                    }
                    j = i;
                }
            }
            else
            {
                for (int i = 0; i < alist.Count; i++)
                {

                    Vector v1 = new Vector((PointD)alist[j], org);
                    Vector v2 = new Vector((PointD)alist[i], (PointD)alist[j]);
                    float sc = MinkDistPtDrt((PointD)alist[j], (PointD)alist[i], org);
                    float crossZ = v1.cross(v2).Z;
                    penetre = penetre && crossZ < 0;
                    if (!penetre) return new Vector();

                    if (sc < maxScalar)
                    {
                        maxScalar = sc;
                        normal = v2.direction();
                        normal = new Vector(-normal.Y, normal.X, 0);
                    }
                    j = i;
                }
            }
            //if (penetre)
            //    //Console.WriteLine("Mincowski : " + maxScalar.ToString());
            return new Vector(normal.X * maxScalar, normal.Y * maxScalar, 0);
        }
        //*/
    }
}
